import os
from pathlib import Path
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
import launch_ros.actions
from launch.substitutions import EnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.conditions import UnlessCondition


def generate_launch_description():
    my_robot_nav_dir = get_package_share_directory('my_robot_nav')
    
    # 配置文件路径
    config_dir = os.path.join(my_robot_nav_dir, 'config')
    ekf_config = os.path.join(config_dir, 'ekf_imu_compensated.yaml')
    ukf_config = os.path.join(config_dir, 'ukf.yaml')
    fuse_config = os.path.join(config_dir, 'fuse_config.yaml')

    # 检查配置文件是否存在
    if not os.path.exists(fuse_config):
        raise FileNotFoundError(f"配置文件不存在: {fuse_config}")

    use_sim_time = LaunchConfiguration('use_sim_time')

    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),

        Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time}, ekf_config],
        ),
        # Node(
        #     package='fuse_optimizers',
        #     executable='fixed_lag_smoother_node',
        #     name='state_estimator',
        #     output='screen',
        #     parameters=[
        #         {'use_sim_time': use_sim_time},
        #         fuse_config
        #     ],
        #     remappings=[
        #         ('odom', '/odom'),
        #         ('imu', '/imu/data'),
        #         ('odom_filtered', '/odom_filtered')
        #     ]
        # ),
    ])
